/*
 * main.cpp
 *
 *  Created on: Mar 22, 2013
 *      Author: ohnozzy
 */
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include "flyingcamera/base/moving/CameraPlanner.h"
#include "flyingcamera/base/CameraSpace.h"
#include "flyingcamera/base/CameraMotionValidator.h"
#include "flyingcamera/base/CameraStateValidityChecker.h"
#include "flyingcamera/base/moving/CameraStateValidityCheckerM.h"
#include "flyingcamera/base/moving/DemoSensor.h"
#include "flyingcamera/gui/GUI.h"
#include "flyingcamera/gui/moving/MCameraGUI.h"
#include "flyingcamera/base/CompoundPath.h"
#include "flyingcamera/base/SimpleSetup.h"

namespace fb = flyingcamera::base;
namespace fg = flyingcamera::gui;
namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace mv = flyingcamera::base::moving;

void demo_static_obstacles(int argc, char** argv)
{
	ob::StateSpacePtr space(new fb::CameraSpace());
	fb::CameraSpace::StateType* temp;
	ob::ScopedState<> start(space);
	temp=start.get()->as<fb::CameraSpace::StateType>();
	temp->setT(0);
    temp->rotation().setIdentity();
    ob::ScopedState<> goal(space);
    temp=goal.get()->as<fb::CameraSpace::StateType>();
    temp->setT(1);
    temp->rotation().setIdentity();
    fb::SimpleSetup ss(space);
    ss.setStartAndGoalStates(start, goal);


    fb::CompoundPath cp;
    fb::StraightLine path1(0, 0, 0.5, 0);
    fb::StraightLine path2(0.5,0, 0.5, 2);
    fb::StraightLine path3(0.5,2, 1, 2);
    cp.addPath(&path1, 1.0/3.0);
    cp.addPath(&path2, 1.0/3.0);
    cp.addPath(&path3, 1-2.0/3.0);
    fb::CameraStateValidityChecker* vali=new fb::CameraStateValidityChecker(ss.getSpaceInformation(), &cp, 1, 0.1);

    fcl::Box b(0.4, 1.6, 1);
    fcl::Box b2(0.4, 1.6, 1);
    fcl::Box b3(1, 0.4, 0.2);
    fcl::Vec3f f1(0.2, 1, 0.5);
    fcl::Vec3f f2(0.8, 1, 0.5);
    fcl::Vec3f f3(0.5, 0.5, 0.3);
    fcl::Vec3f c1(1, 0, 0);
    fcl::Vec3f c2(0, 1, 0);
    fcl::Vec3f c3(0, 0, 1);
    vali->addObstacle(&b, fcl::Transform3f(f1));
    vali->addObstacle(&b2, fcl::Transform3f(f2));
    vali->addObstacle(&b3, fcl::Transform3f(f3));
    //Moving Obstacle
    fb::CompoundPath mo_p;
    fb::StraightLine mo_path1(0.2, 0, 0.5, 0);
    fb::StraightLine mo_path2(0.5,0, 0.5, 2);
    fb::StraightLine mo_path3(0.5,2, 0, 2);
    mo_p.addPath(&mo_path1, 0.4/3.0);
    mo_p.addPath(&mo_path2, 1.0/3.0);
    mo_p.addPath(&mo_path3, 1-1.4/3.0);
    fcl::Vec3f mo_c(0, 0.5, 0.5);
    fcl::Box mo(0.1,0.1,0.1);

    ss.setStateValidityChecker(ob::StateValidityCheckerPtr(vali));
    ss.setPlanner(ob::PlannerPtr(new og::RRT(ss.getSpaceInformation())));


    ob::PlannerStatus solved=ss.solve(30);
    if(solved){
    	ss.getSolutionPath().print(std::cout);
    }
    fg::moving::MCameraGUI g(ss.getSolutionPath(),&cp,1);
    g.addObstacle(b, f1, c1);
    g.addObstacle(b2, f2, c2);
    g.addObstacle(b3, f3, c3);
    g.addMObstacle(mo, &mo_p, mo_c);
    fg::setCameraGUI(&g);
    fg::show(&argc, argv);
}

void demo_moving_obstacles(int argc, char** argv)
{
	ob::StateSpacePtr space(new fb::CameraSpace());
	fb::CameraSpace::StateType* temp;
	ob::ScopedState<> start(space);
	temp=start.get()->as<fb::CameraSpace::StateType>();
	temp->setT(0);
    temp->rotation().setIdentity();
    ob::ScopedState<> goal(space);
    temp=goal.get()->as<fb::CameraSpace::StateType>();
    temp->setT(1);
    temp->rotation().setIdentity();
    fb::SimpleSetup ss(space);
    ss.setStartAndGoalStates(start, goal);


    fb::CompoundPath cp;
    fb::StraightLine path1(0, 0, 0.5, 0);
    fb::StraightLine path2(0.5,0, 0.5, 2);
    fb::StraightLine path3(0.5,2, 1, 2);
    cp.addPath(&path1, 1.0/3.0);
    cp.addPath(&path2, 1.0/3.0);
    cp.addPath(&path3, 1.0/3.0);
    
    mv::CameraStateValidityCheckerM* vali=new mv::CameraStateValidityCheckerM(ss.getSpaceInformation(), &cp, 1, 0.1);

    fcl::Box b(0.4, 1.6, 1);
    fcl::Box b2(0.4, 1.6, 1);
    fcl::Box b3(1, 0.4, 0.2);
    fcl::Vec3f f1(0.2, 1, 0.5);
    fcl::Vec3f f2(0.8, 1, 0.5);
    fcl::Vec3f f3(0.5, 0.5, 0.3);
    fcl::Vec3f c1(1, 0, 0);
    fcl::Vec3f c2(0, 1, 0);
    fcl::Vec3f c3(0, 0, 1);
    //fcl::Box mo(0.1,0.05,0.2);
    //fcl::Vec3f f4(0.55, 0.519278, 0.1);
    vali->addObstacle(&b, fcl::Transform3f(f1));
    vali->addObstacle(&b2, fcl::Transform3f(f2));
    vali->addObstacle(&b3, fcl::Transform3f(f3));
    //vali->addObstacle(&mo, fcl::Transform3f(f4));
 	
 	//Moving Obstacle
    fb::CompoundPath mo_p;
    fb::StraightLine mo_path1(0, 0.3, 0, 0);
    fb::StraightLine mo_path2(0,0, 0.55, 0);
    fb::StraightLine mo_path3(0.55,0, 0.55, 2);
    fb::StraightLine mo_path4(0.55,2, 0.8, 2);
    mo_p.addPath(&mo_path1, 0.3/3.0);
    mo_p.addPath(&mo_path2, 1/3.0);
    mo_p.addPath(&mo_path3, 1/3.0);
    mo_p.addPath(&mo_path4, 0.6/3.0);
    fcl::Vec3f mo_c(0, 0.3, 0);
    fcl::Box mo(0.1,0.05,0.2);
    fcl::Transform3f mo_f(mo_c);
    
    ss.setStateValidityChecker(ob::StateValidityCheckerPtr(vali));

    mv::CameraPlanner cPlanner(ss.getSpaceInformation());
    ss.setPlanner(ob::PlannerPtr(&cPlanner));
    ss.setup();
    mv::DemoSensor* sensor = new mv::DemoSensor();
    sensor->addMObstacle(&mo,  &mo_p);
    sensor->setTime(0);

    ob::PlannerStatus hasNextStep(false, false);
    bool hasSolution = false;

    do
    {
    	sensor->getNearObstacles(vali);
    	hasNextStep = cPlanner.solve(30.0);
    	hasSolution = cPlanner.isSolutionFound();
    	sensor->setTime(cPlanner.getTime());    	
    } while ((!hasSolution) && hasNextStep);
	
    /*if (hasSolution)
    	ss.getSolutionPath().print(std::cout);*/

    fg::moving::MCameraGUI g(ss.getSolutionPath(),&cp,1);
    g.addObstacle(b, f1, c1);
    g.addObstacle(b2, f2, c2);
    g.addObstacle(b3, f3, c3);
    g.addMObstacle(mo, &mo_p, mo_c);
    //g.addObstacle(mo, f4, mo_c);
    
    fg::setCameraGUI(&g);
    fg::show(&argc, argv);
}

int main(int argc, char** argv){
	//demo_static_obstacles(argc, argv);
	demo_moving_obstacles(argc, argv);
}
